#pragma once

#include "xtd/tree2d.h"

namespace xtd {

template <class T, class P, class R, class C>
class tree2d<T,P,R,C>::node {
public:
	size_type	parent; // index of parent node
	size_type	left;	// index of left child
	size_type	right;	// index of right child
	value_type	value;	// data
	bool		xnode;	// splits space by x, otherwise by y
	bool		removed; // deletion mark
public:
	node(value_type v = value_type())
		: parent(npos), left(npos), right(npos), value(v), xnode(true), removed(false) 
		{ }
	bool less(const node & n, const cast_type & cast) const 
	{
		point_type p(cast(value)), q(cast(n.value));
		return xnode ? p.x < q.x : p.y < q.y; 
	}
	bool left_in(const rect_type & r, const cast_type & cast) const 
	{
		if (left == npos)
			return false;
		point_type p(cast(value));
		return xnode ? p.x > r.x1 : p.y > r.y1;
	}
	bool right_in(const rect_type & r, const cast_type & cast) const 
	{
		if (right == npos)
			return false;
		point_type p(cast(value));
		return xnode ? p.x < r.x2 : p.y < r.y2;
	}
};

template <class T, class P, class R, class C>
class tree2d<T,P,R,C>::less_x {
private:
	const cast_type & cast;
public:
	less_x(const cast_type & c)
		: cast(c) { }
	bool operator () (const node & m, const node & n) 
	{
		point_type p(cast(m.value)), q(cast(n.value));
		return p.x < q.x; 
	}
};

template <class T, class P, class R, class C>
class tree2d<T,P,R,C>::less_y {
private:
	const cast_type & cast;
public:
	less_y(const cast_type & c)
		: cast(c) { }
	bool operator () (const node & m, const node & n) 
	{
		point_type p(cast(m.value)), q(cast(n.value));
		return p.y < q.y; 
	}
};	 

template <class T, class P, class R, class C>
template <class D> 
class tree2d<T,P,R,C>::nearest_locator {
public:
	typedef typename D::result_type distance_type;
public:
	nearest_locator(tree2d const & t, cast_type const & c, point_type const & p, 
		D const & rangefinder, distance_type distance_threshold) 
		:
		tree(t), cast(c), target(p), range_finder(rangefinder), best_found(npos), up_bound(distance_threshold) 
		{ }
	size_type find() {
		search(tree._root); 
		return best_found;
	}
	void search(size_type root) 
	{
		if (root == npos)
			return;
		node const & nd = tree._nodes[root];
		point_type p = cast(nd.value);
		distance_type c1 = nd.xnode ? target.x : target.y;
		distance_type c2 = nd.xnode ? p.x : p.y;
		//distance_type dc = range_finder(c1, c2);
		//if (dc < up_bound) {
			distance_type t = range_finder(target, p);
			if (t < up_bound) {
				best_found = root;
				up_bound = t;
			}
		//}

		if (c1 < c2 + up_bound)
			search(nd.left);
		if (c2 < c1 + up_bound)
			search(nd.right);
	}
private:
	tree2d const & tree;
	cast_type const & cast;
	point_type const & target;
	D const & range_finder;
	size_type best_found;
	distance_type up_bound;
};

template <class T, class P, class R, class C>
class tree2d<T,P,R,C>::range_iterator {
public:
	const node_vector *	ptr;
	const cast_type *	pcast;
	size_type			index;
	rect_type			range;
private:
	// sets new index and returns previous value
	size_type set_index(size_type i) 
		{ size_type tmp(index);	index = i; return tmp; }
	const node & current() const 
		{ return (*ptr)[index]; }
	// returns true if current node fits in range
	bool fits() const { 
		return !current().removed && 
			range.inside((* pcast)(current().value)); 
	}
	// starts from root and stops at the first node in range
	void start(size_type root) 
	{
		if ((index = root) != npos && !fits())
			next();
	}
	// proceeds to the next node in range
	void next() 
	{
		size_type last = index; // last visited
		while (index != npos) {
			if (current().left != last && current().left_in(range, * pcast))
				last = set_index(current().left);
			else {
				while (current().right == last || !current().right_in(range, * pcast)) {
					last = set_index(current().parent);
					if (index == npos)
						return;
				}
				last = set_index(current().right);
			}
			if (fits())
				return;
		}
	}
public:
	range_iterator() 
		: ptr(0), pcast(0), index(npos)
		{ }
	range_iterator(const node_vector * p, const cast_type * pc, size_type i)
		: ptr(p), pcast(pc), index(i)
		{ }
	range_iterator(const node_vector * p, const cast_type * pc, size_type root, const rect_type & r)
		: ptr(p), pcast(pc), index(npos), range(r) 
		{ start(root); }			
	range_iterator(const range_iterator & it)
		: ptr(it.ptr), pcast(it.pcast), index(it.index), range(it.range)
		{ }
	bool operator == (const range_iterator & it) const
		{ return ptr == it.ptr && index == it.index; }
	bool operator != (const range_iterator & it) const
		{ return ptr != it.ptr || index != it.index; }
	const value_type & operator * ()
		{ return current().value; }
	const value_type * operator -> ()
		{ return &current().value; }
	range_iterator & operator ++ ()
		{ next(); return * this; }
};

template <class T, class P, class R, class C>
class tree2d<T,P,R,C>::iterator {
public:
	node_vector *	ptr;
	size_type		index;
public:
	iterator()
		: ptr(0), index(npos)
		{ }
	iterator(node_vector * p, size_type i)
		: ptr(p), index(i)
		{ }
	iterator(const iterator & it)
		: ptr(it.ptr), index(it.index)
		{ }
	iterator(const range_iterator & it)
		: ptr(const_cast<node_vector *>(it.ptr)), index(it.index)
		{ }
	bool operator == (const iterator & it) const
		{ return ptr == it.ptr && index == it.index; }
	bool operator != (const iterator & it) const
		{ return ptr != it.ptr || index != it.index; }
	value_type & operator * ()
		{ return (*ptr)[index].value; }
	iterator & operator ++ ()
		{ while(++index < ptr->size() && (*ptr)[index].removed); return * this; }
};

template <class T, class P, class R, class C>
class tree2d<T,P,R,C>::const_iterator {
public:
	const node_vector *	ptr;
	size_type			index;
public:
	const_iterator()
		: ptr(0), index(npos)
		{ }
	const_iterator(const node_vector * p, size_type i)
		: ptr(p), index(i)
		{ }
	const_iterator(const iterator & it)
		: ptr(it.ptr), index(it.index)
		{ }
	const_iterator(const const_iterator & it)
		: ptr(it.ptr), index(it.index)
		{ }
	const_iterator(const range_iterator & it)
		: ptr(it.ptr), index(it.index)
		{ }
	bool operator == (const const_iterator & it) const
		{ return ptr == it.ptr && index == it.index; }
	bool operator != (const const_iterator & it) const
		{ return ptr != it.ptr || index != it.index; }
	const value_type & operator * ()
		{ return (*ptr)[index].value; }
	const_iterator & operator ++ ()
		{ while(++index < ptr->size() && (*ptr)[index].removed); return * this; }
};

template <class T, class P, class R, class C>
typename tree2d<T,P,R,C>::size_type tree2d<T,P,R,C>::subtree(size_type first, size_type last, bool xnode) 
{
	if (first == last)
		return npos;
	if (xnode)
		std::sort(_nodes.begin() + first, _nodes.begin() + last, less_x(_cast));
	else
		std::sort(_nodes.begin() + first, _nodes.begin() + last, less_y(_cast));
	size_type mid = (last + first) / 2;
	node & n = _nodes[mid];
	n.xnode = xnode;
	n.left = subtree(first, mid, !xnode);
	n.right = subtree(mid + 1, last, !xnode);
	if (n.left != npos)
		_nodes[n.left].parent = mid;
	if (n.right != npos)
		_nodes[n.right].parent = mid;
	return mid;
}

template <class T, class P, class R, class C>
void tree2d<T,P,R,C>::insert(size_type root, size_type index)
{
	node & r = _nodes[root]; 
	node & n = _nodes[index];
	if (r.less(n, _cast)) {
		if (r.right != npos)
			insert(r.right, index);
		else {
			r.right = index;
			n.parent = root;
			n.xnode = !r.xnode;
		}
	} else {
		if (r.left != npos)
			insert(r.left, index);
		else {
			r.left = index;
			n.parent = root;
			n.xnode = !r.xnode;
		}
	}
}

//regular tree2d methods

template <class T, class P, class R, class C>
inline tree2d<T,P,R,C>::tree2d(const cast_type & c)
	: _root(npos), _removed(0), _cast(c)
	{ }

template <class T, class P, class R, class C>
template <class I>
inline tree2d<T,P,R,C>::tree2d(I first, I last, const cast_type & c)
	: _root(npos), _removed(0), _cast(c)
	{ assign(first, last); }

template <class T, class P, class R, class C>
inline tree2d<T,P,R,C>::~tree2d()
	{ }

template <class T, class P, class R, class C>
inline typename tree2d<T,P,R,C>::iterator tree2d<T,P,R,C>::begin()
	{ return iterator(&_nodes, 0); }

template <class T, class P, class R, class C>
inline typename tree2d<T,P,R,C>::iterator tree2d<T,P,R,C>::end()
	{ return iterator(&_nodes, _nodes.size()); }

template <class T, class P, class R, class C>
inline typename tree2d<T,P,R,C>::const_iterator tree2d<T,P,R,C>::begin() const
	{ return const_iterator(&_nodes, 0); }

template <class T, class P, class R, class C>
inline typename tree2d<T,P,R,C>::const_iterator tree2d<T,P,R,C>::end() const
	{ return const_iterator(&_nodes, _nodes.size()); }

template <class T, class P, class R, class C>
inline typename tree2d<T,P,R,C>::range_iterator tree2d<T,P,R,C>::range_begin(const rect_type & r) const
	{ return range_iterator(&_nodes, &_cast, _root, r); }

template <class T, class P, class R, class C>
inline typename tree2d<T,P,R,C>::range_iterator tree2d<T,P,R,C>::range_end() const
	{ return range_iterator(&_nodes, &_cast, npos); } 

template <class T, class P, class R, class C>
inline bool tree2d<T,P,R,C>::empty() const
	{ return size() == 0; }

template <class T, class P, class R, class C>
inline typename tree2d<T,P,R,C>::size_type tree2d<T,P,R,C>::size() const
	{ return _nodes.size() - _removed; }

template <class T, class P, class R, class C>
inline typename tree2d<T,P,R,C>::size_type tree2d<T,P,R,C>::capacity() const
	{ return _nodes.capacity() - _removed; }

template <class T, class P, class R, class C>
inline typename tree2d<T,P,R,C>::cast_type & tree2d<T,P,R,C>::cast() const
	{ return _cast; }

template <class T, class P, class R, class C>
inline void tree2d<T,P,R,C>::push_back(const value_type & v)
{
	_nodes.push_back(node(v));
	if (_root != npos)
		insert(_root, size_type(_nodes.size() - 1));
	else
		_root = 0;
}

template <class T, class P, class R, class C>
template <class I>
inline void tree2d<T,P,R,C>::assign(I first, I last)
{
	clear();
	reserve(std::distance(first, last));
	for (; first != last; ++first)
		push_back(*first);
	rebalance();
}

template <class T, class P, class R, class C>
inline void tree2d<T,P,R,C>::erase(const_iterator it)
{
	_nodes[it.index].removed = true;
	++ _removed;
}

template <class T, class P, class R, class C>
inline void tree2d<T,P,R,C>::clear()
{
	_nodes.clear();
	_removed = 0;
	_root = npos;
}

template <class T, class P, class R, class C>
inline void tree2d<T,P,R,C>::reserve(size_type n)
{
	_nodes.reserve(n + _removed);
}

template <class T, class P, class R, class C>
void tree2d<T,P,R,C>::rebalance()
{
 	size_type from = 0, to = 0;
	for (; from < _nodes.size(); ++from) {
		if (!_nodes[from].removed) {
			if (to != from)
				_nodes[to] = _nodes[from];
			++to;
		}
	}
	_nodes.erase(_nodes.begin() + to, _nodes.end());
	_removed = 0;
	_root = subtree(0, size_type(_nodes.size()), true);
	if (_root != npos)
		_nodes[_root].parent = npos;
}

template <class T, class P, class R, class C>
template <class D>
inline typename tree2d<T,P,R,C>::const_iterator tree2d<T,P,R,C>::nearest(
	const point_type & p, D const & range_finder,
	typename D::result_type distance_threshold) const
{
	return const_iterator(&_nodes, 
		nearest_locator<D>(* this, _cast, p, range_finder, distance_threshold).find());
}

} //namespace xtd